using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.DirectX;

namespace Noein.Physics
{
    public class Physical
    {
        #region Linear

        /// <summary>
        /// v = ds/dt, (m/s)
        /// </summary>
        private Vector3 v = Vector3.Empty;
        public Vector3 Velocity
        {
           get { return v; }
        }

        /// <summary>
        /// a = dv/dt = d^2s/dt^2, (m/s^2)
        /// </summary>
        private Vector3 a = Vector3.Empty;
        public Vector3 Acceleration
        {
            get { return a; }
        }

        /// <summary>
        /// p = mv, (kg*m/s)
        /// </summary>
        private Vector3 p = Vector3.Empty;
        public Vector3 Momentum
        {
            get { return p; }
            set { p = value; v = value * OneOverMass; }
        }

        /// <summary>
        /// F = ma, (newton, kg*m/s^2)
        /// </summary>
        private Vector3 F = Vector3.Empty;
        public Vector3 Force
        {
            get { return F; }
            set { F = value; a = value * oneOverMass; }
        }

        private Vector3 customForce = Vector3.Empty;
        public Vector3 CustomForce
        {
            get { return customForce; }
            set { customForce = value; }
        }

        #endregion
        #region Rotational

        /// <summary>
        /// 2nd-rank tensor form
        /// </summary>
        private Matrix I = Matrix.Identity;
        public Matrix MomentOfInertia
        {
            get { return I; }
            set { I = value; }
        }

        private Vector3 omega = Vector3.Empty;
        public Vector3 AngularVelocity
        {
            get { return omega; }
            set { omega = value; }
        }

        private Vector3 alpha = Vector3.Empty;
        public Vector3 AngularAcceleration
        {
            get { return alpha; }
            set { alpha = value; }
        }

        private Vector3 L = Vector3.Empty;
        public Vector3 AngularMomentum
        {
            get { return L; }
            set { L = value; }
        }
        
        /// <summary>
        /// torque, aka moment ???
        /// tau = (r - x) cross F, x = COM, r = point where force is applied, F = force applied
        /// newton metres
        /// </summary>
        private Vector3 tau = Vector3.Empty;
        public Vector3 Torque
        {
            get { return tau; }
            set { tau = value; }
        }

        #endregion
        #region General

        /// <summary>
        /// m = F/a = M/v,(kg)
        /// </summary>
        private float mass;
        private float oneOverMass;
        public float Mass
        {
            get { return mass; }
            set { mass = value; oneOverMass = 1f / mass; }
        }
        public float OneOverMass
        {
            get { return oneOverMass; }
        }

        /// <summary>
        /// Center of mass
        /// </summary>
        //private Vector3 centroid;
        //public Vector3 Centroid
        //{
        //    get { return centroid; }
        //    set { centroid = value; }
        //}

        private Spatial spatial;
        public Vector3 Position
        {
            get { return spatial.Position; }
            set { spatial.MoveTo(value); }
        }

        ///// <summary>
        ///// ????
        ///// position to center of mass???
        ///// </summary>
        //private Vector3 relativePosition;
        //public Vector3 RelativePosition
        //{
        //    get { return relativePosition; }
        //    set { relativePosition = value; }
        //}

        private Geometry geometry = new Sphere(1f);
        public Geometry Geometry
        {
            get { return geometry; }
        }
        //private Geometry geometry = new Geometry.RectangularCylinder();
        //public Geometry Geometry
        //{
        //    get { return geometry; }
        //    set 
        //    {
        //        geometry = value;
        //        float[] principle = Functions.MassMomentsOfInertia(geometry, mass, dimensions);
        //        Matrix newInertia = Matrix.Identity;
        //        newInertia.M11 = principle[0];
        //        newInertia.M22 = principle[1];
        //        newInertia.M33 = principle[2];
        //        MomentOfInertia = newInertia;
        //    }
        //}

        //// TODO this is no good
        //private float[] dimensions = new float[] { 1, 1, 1 };
        //public float[] Dimensions
        //{
        //    get { return dimensions; }
        //    set { dimensions = value; }
        //}

        public Vector3 contact = Vector3.Empty;

        #endregion
        #region Creation

        public Physical(Spatial spatial)
            : this(1f, spatial, Vector3.Empty, Vector3.Empty)
        {
        }

        // TODO get rid of initial acceleration, assign permanent force to physics controller
        public Physical(float mass, Spatial spatial, Vector3 initialVelocity, Vector3 initialAcceleration)
        {
            Mass = mass;

            this.spatial = spatial;

            // derive momentum from velocity
            v = initialVelocity;
            p = mass * initialVelocity;

            // derive force from acceleration
            a = initialAcceleration;
            F = mass * initialAcceleration;
        }

        public Physical(Physical physical)
            : this(physical.Mass, physical.spatial, physical.Velocity, physical.Acceleration)
        {
        }

        #endregion
        #region Helper Accessors

        //public Quaternion QuatAngularVelocity(Quaternion orientation, Matrix orientationMatrix)
        //{
        //    // TODO is transforming angular velocity to world first needed?
        //    Vector3 angularVelocity = Vector3.TransformCoordinate(AngularVelocity, orientationMatrix);
        //    Quaternion retVal = new Quaternion(angularVelocity.X, angularVelocity.Y, angularVelocity.Z, 0);
        //    retVal = Quaternion.Multiply(retVal, orientation);
        //    retVal.W *= .5f;
        //    retVal.X *= .5f;
        //    retVal.Y *= .5f;
        //    retVal.Z *= .5f;
        //    return retVal;
        //}

        //public Matrix WorldMomentOfInertia(Matrix transform)
        //{
        //    Matrix retVal = Matrix.Multiply(transform, MomentOfInertia);
        //    retVal.Multiply(Matrix.TransposeMatrix(transform));
        //    return retVal;
        //}

        //public Matrix InverseWorldMomentOfInertia(Matrix transform)
        //{

        //    Matrix retVal = transform;
        //    retVal.Multiply(MomentOfInertia);
        //    retVal.Multiply(Matrix.TransposeMatrix(transform));
        //    return retVal;
        //}

        //public Vector3 TangentialVelocity(Matrix transform)
        //{
        //    // BUG wrong
        //    return Vector3.TransformCoordinate(Vector3.Cross(AngularVelocity, RelativePosition), transform);
        //}

        //public Vector3 TotalLinearVelocity(Matrix transform)
        //{
        //    return Velocity + TangentialVelocity(transform);
        //}

        #endregion

        /// <summary>
        /// Computing the physics properties of a body that's made up of multiple components
        /// </summary>
        /// <param name="components">components' physics properties</param>
        /// <returns></returns>
        //public static PhysicsInfo Combine(params PhysicsInfo[] components)
        //{
        //    PhysicsInfo retVal = new PhysicsInfo();

        //    foreach (PhysicsInfo component in components)
        //    {
        //        // compute total mass
        //        retVal.Mass += component.Mass;

        //        // NOTE assuming all positions are in the same 3D space
        //        // compute centroid
        //        retVal.Position += component.Mass * component.Position;
        //    }

        //    retVal.Position *= 1f / retVal.Mass;

        //    #region Inertia calculation
        //    Matrix momentOfInertia = new Matrix();
        //    foreach (PhysicsInfo component in components)
        //    {
        //        // set component position relative to the body's centroid
        //        component.position -= retVal.position;

        //        // principle axis terms
        //        momentOfInertia.M11 += component.MomentOfInertia.M11 + component.Mass *
        //            (component.Position.Y * component.Position.Y + component.Position.Z * component.Position.Z);
        //        momentOfInertia.M22 += component.MomentOfInertia.M22 + component.Mass *
        //            (component.Position.Z * component.Position.Z + component.Position.X * component.Position.X);
        //        momentOfInertia.M33 += component.MomentOfInertia.M33 + component.Mass *
        //            (component.Position.X * component.Position.X + component.Position.Y * component.Position.Y);
                
        //        // product of inertia terms
        //        momentOfInertia.M12 += component.MomentOfInertia.M12 + component.Mass *
        //            (component.Position.X * component.Position.Y);
        //        momentOfInertia.M13 += component.MomentOfInertia.M13 + component.Mass *
        //            (component.Position.X * component.Position.Z);
        //        momentOfInertia.M23 += component.MomentOfInertia.M23 + component.Mass *
        //            (component.Position.Y * component.Position.Z);
        //    }
        //    momentOfInertia.M21 = momentOfInertia.M12 = -momentOfInertia.M12;
        //    momentOfInertia.M31 = momentOfInertia.M13 = -momentOfInertia.M13;
        //    momentOfInertia.M32 = momentOfInertia.M23 = -momentOfInertia.M23;

        //    retVal.MomentOfInertia = momentOfInertia;
        //    #endregion

        //    return retVal;
        //}
    }

    // TODO
    // public class 2DPhysical : Physical

    // TODO
    // public class TranslationalPhysical : Physical

    // TODO
    // public class RigidBodyPhysical : TranslationalPhysical
}
